//////////////////////////////////////////////////////////////////////////
// Copyright 2023 The Aerospace Corporation
//
// This file is part of SatCat5.
//
// SatCat5 is free software: you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License as published by the
// Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
//
// SatCat5 is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
// License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with SatCat5.  If not, see <https://www.gnu.org/licenses/>.
//////////////////////////////////////////////////////////////////////////

#include <hal_devices/i2c_ssd1306.h>

using satcat5::cfg::I2cGeneric;
using satcat5::device::i2c::Ssd1306;
using satcat5::util::I2cAddr;

// Define various hardware constants:
constexpr I2cAddr DEV_ADDR = I2cAddr::addr8(0x78);
constexpr u8 CMD_NOARG   = 0x80;    // Command/opcode
constexpr u8 CMD_ARG     = 0x00;    // Command/opcode/arg/arg...
constexpr u8 CMD_PIXEL   = 0x40;    // Command/data/data/data/...

// Commmand opcodes have 0, 1, or 2 arguments.
struct Opcode {
    u8 nbytes;
    u32 data;

    explicit constexpr Opcode(u8 opcode)
        : nbytes(2), data(256*CMD_NOARG + opcode) {}
    constexpr Opcode(u8 opcode, u8 arg1)
        : nbytes(3), data(65536*CMD_ARG + 256*opcode + arg1) {}
    constexpr Opcode(u8 opcode, u8 arg1, u8 arg2)
        : nbytes(4), data(16777216*CMD_ARG + 65536*opcode + 256*arg1 + arg2) {}
};

// Initial startup sequence.
constexpr Opcode SSD1306_STARTUP[] = {
    Opcode(0xAE),           // Display OFF
    Opcode(0xD5, 0x80),     // Clock divide = Default
    Opcode(0xA8, 0x1F),     // Multiplexing = 32 rows
    Opcode(0xD3, 0x00),     // No display offset
    Opcode(0x40),           // Start line = 0
    Opcode(0x8D, 0x14),     // Charge pump
    Opcode(0x20, 0x00),     // Memory mode = Horizontal
    Opcode(0xA1),           // Horizontal mirroring
    Opcode(0xC8),           // Vertical mirroring
    Opcode(0xDA, 0x02),     // COM pin configuration
    Opcode(0x81, 0x8F),     // Contrast
    Opcode(0xD9, 0xF1),     // Precharge period
    Opcode(0xDB, 0x40),     // VCOMH threshold
    Opcode(0xA4),           // Normal readout mode
    Opcode(0xA6),           // Non-inverted display
    Opcode(0xAF),           // Display ON
};
constexpr unsigned SSD1306_STEP_DRAW =
    sizeof(SSD1306_STARTUP) / sizeof(Opcode);
constexpr unsigned SSD1306_STEP_DATA =
    SSD1306_STEP_DRAW + 2;
constexpr unsigned SSD1306_STEP_SWAP =
    SSD1306_STEP_DATA + 64;
constexpr unsigned SSD1306_STEP_DONE =
    SSD1306_STEP_SWAP + 1;

// Additional commands used to draw each frame.
constexpr Opcode SSD1306_PAGE_RESET(0x21, 0, 127);
constexpr Opcode SSD1306_PAGE_START0(0x22, 0, 3);
constexpr Opcode SSD1306_PAGE_START4(0x22, 4, 7);
constexpr Opcode SSD1306_PAGE_END0(0x40);
constexpr Opcode SSD1306_PAGE_END4(0x60);

// A simple 8x8 font in the preferred format for the SSD1306.
// Each byte is one column, with the bottom pixel in the MSB.
// Derived from Daniel Hepper's public domain bitmap font:
// https://github.com/dhepper/font8x8/blob/master/font8x8_basic.h
struct FontCharacter {u8 cols[8];};
constexpr char SSD1306_FONT_MIN = 0x20;
constexpr char SSD1306_FONT_MAX = 0x7E;
constexpr FontCharacter SSD1306_FONT[95] = {
    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // (space) = 0x20
    {0x00, 0x00, 0x06, 0x5F, 0x5F, 0x06, 0x00, 0x00},   // ! = 0x21
    {0x00, 0x03, 0x03, 0x00, 0x03, 0x03, 0x00, 0x00},   // " = 0x22
    {0x14, 0x7F, 0x7F, 0x14, 0x7F, 0x7F, 0x14, 0x00},   // # = 0x23
    {0x24, 0x2E, 0x6B, 0x6B, 0x3A, 0x12, 0x00, 0x00},   // $ = 0x24
    {0x46, 0x66, 0x30, 0x18, 0x0C, 0x66, 0x62, 0x00},   // % = 0x25
    {0x30, 0x7A, 0x4F, 0x5D, 0x37, 0x7A, 0x48, 0x00},   // & = 0x26
    {0x04, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00},   // ' = 0x27
    {0x00, 0x1C, 0x3E, 0x63, 0x41, 0x00, 0x00, 0x00},   // ( = 0x28
    {0x00, 0x41, 0x63, 0x3E, 0x1C, 0x00, 0x00, 0x00},   // ) = 0x29
    {0x08, 0x2A, 0x3E, 0x1C, 0x1C, 0x3E, 0x2A, 0x08},   // * = 0x2A
    {0x08, 0x08, 0x3E, 0x3E, 0x08, 0x08, 0x00, 0x00},   // + = 0x2B
    {0x00, 0x80, 0xE0, 0x60, 0x00, 0x00, 0x00, 0x00},   // , = 0x2C
    {0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00},   // - = 0x2D
    {0x00, 0x00, 0x60, 0x60, 0x00, 0x00, 0x00, 0x00},   // . = 0x2E
    {0x60, 0x30, 0x18, 0x0C, 0x06, 0x03, 0x01, 0x00},   // / = 0x2F
    {0x3E, 0x7F, 0x71, 0x59, 0x4D, 0x7F, 0x3E, 0x00},   // 0 = 0x30
    {0x40, 0x42, 0x7F, 0x7F, 0x40, 0x40, 0x00, 0x00},   // 1 = 0x31
    {0x62, 0x73, 0x59, 0x49, 0x6F, 0x66, 0x00, 0x00},   // 2 = 0x32
    {0x22, 0x63, 0x49, 0x49, 0x7F, 0x36, 0x00, 0x00},   // 3 = 0x33
    {0x18, 0x1C, 0x16, 0x53, 0x7F, 0x7F, 0x50, 0x00},   // 4 = 0x34
    {0x27, 0x67, 0x45, 0x45, 0x7D, 0x39, 0x00, 0x00},   // 5 = 0x35
    {0x3C, 0x7E, 0x4B, 0x49, 0x79, 0x30, 0x00, 0x00},   // 6 = 0x36
    {0x03, 0x03, 0x71, 0x79, 0x0F, 0x07, 0x00, 0x00},   // 7 = 0x37
    {0x36, 0x7F, 0x49, 0x49, 0x7F, 0x36, 0x00, 0x00},   // 8 = 0x38
    {0x06, 0x4F, 0x49, 0x69, 0x3F, 0x1E, 0x00, 0x00},   // 9 = 0x39
    {0x00, 0x00, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00},   // : = 0x3A
    {0x00, 0x80, 0xE6, 0x66, 0x00, 0x00, 0x00, 0x00},   // ; = 0x3B
    {0x08, 0x1C, 0x36, 0x63, 0x41, 0x00, 0x00, 0x00},   // < = 0x3C
    {0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x00, 0x00},   // = = 0x3D
    {0x00, 0x41, 0x63, 0x36, 0x1C, 0x08, 0x00, 0x00},   // > = 0x3E
    {0x02, 0x03, 0x51, 0x59, 0x0F, 0x06, 0x00, 0x00},   // ? = 0x3F
    {0x3E, 0x7F, 0x41, 0x5D, 0x5D, 0x1F, 0x1E, 0x00},   // @ = 0x40
    {0x7C, 0x7E, 0x13, 0x13, 0x7E, 0x7C, 0x00, 0x00},   // A = 0x41
    {0x41, 0x7F, 0x7F, 0x49, 0x49, 0x7F, 0x36, 0x00},   // B = 0x42
    {0x1C, 0x3E, 0x63, 0x41, 0x41, 0x63, 0x22, 0x00},   // C = 0x43
    {0x41, 0x7F, 0x7F, 0x41, 0x63, 0x3E, 0x1C, 0x00},   // D = 0x44
    {0x41, 0x7F, 0x7F, 0x49, 0x5D, 0x41, 0x63, 0x00},   // E = 0x45
    {0x41, 0x7F, 0x7F, 0x49, 0x1D, 0x01, 0x03, 0x00},   // F = 0x46
    {0x1C, 0x3E, 0x63, 0x41, 0x51, 0x73, 0x72, 0x00},   // G = 0x47
    {0x7F, 0x7F, 0x08, 0x08, 0x7F, 0x7F, 0x00, 0x00},   // H = 0x48
    {0x00, 0x41, 0x7F, 0x7F, 0x41, 0x00, 0x00, 0x00},   // I = 0x49
    {0x30, 0x70, 0x40, 0x41, 0x7F, 0x3F, 0x01, 0x00},   // J = 0x4A
    {0x41, 0x7F, 0x7F, 0x08, 0x1C, 0x77, 0x63, 0x00},   // K = 0x4B
    {0x41, 0x7F, 0x7F, 0x41, 0x40, 0x60, 0x70, 0x00},   // L = 0x4C
    {0x7F, 0x7F, 0x0E, 0x1C, 0x0E, 0x7F, 0x7F, 0x00},   // M = 0x4D
    {0x7F, 0x7F, 0x06, 0x0C, 0x18, 0x7F, 0x7F, 0x00},   // N = 0x4E
    {0x1C, 0x3E, 0x63, 0x41, 0x63, 0x3E, 0x1C, 0x00},   // O = 0x4F
    {0x41, 0x7F, 0x7F, 0x49, 0x09, 0x0F, 0x06, 0x00},   // P = 0x50
    {0x1E, 0x3F, 0x21, 0x71, 0x7F, 0x5E, 0x00, 0x00},   // Q = 0x51
    {0x41, 0x7F, 0x7F, 0x09, 0x19, 0x7F, 0x66, 0x00},   // R = 0x52
    {0x26, 0x6F, 0x4D, 0x59, 0x73, 0x32, 0x00, 0x00},   // S = 0x53
    {0x03, 0x41, 0x7F, 0x7F, 0x41, 0x03, 0x00, 0x00},   // T = 0x54
    {0x7F, 0x7F, 0x40, 0x40, 0x7F, 0x7F, 0x00, 0x00},   // U = 0x55
    {0x1F, 0x3F, 0x60, 0x60, 0x3F, 0x1F, 0x00, 0x00},   // V = 0x56
    {0x7F, 0x7F, 0x30, 0x18, 0x30, 0x7F, 0x7F, 0x00},   // W = 0x57
    {0x43, 0x67, 0x3C, 0x18, 0x3C, 0x67, 0x43, 0x00},   // X = 0x58
    {0x07, 0x4F, 0x78, 0x78, 0x4F, 0x07, 0x00, 0x00},   // Y = 0x59
    {0x47, 0x63, 0x71, 0x59, 0x4D, 0x67, 0x73, 0x00},   // Z = 0x5A
    {0x00, 0x7F, 0x7F, 0x41, 0x41, 0x00, 0x00, 0x00},   // [ = 0x5B
    {0x01, 0x03, 0x06, 0x0C, 0x18, 0x30, 0x60, 0x00},   // (backslash)
    {0x00, 0x41, 0x41, 0x7F, 0x7F, 0x00, 0x00, 0x00},   // ] = 0x5D
    {0x08, 0x0C, 0x06, 0x03, 0x06, 0x0C, 0x08, 0x00},   // ^ = 0x5E
    {0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80},   // _ = 0x5F
    {0x00, 0x00, 0x03, 0x07, 0x04, 0x00, 0x00, 0x00},   // ` = 0x60
    {0x20, 0x74, 0x54, 0x54, 0x3C, 0x78, 0x40, 0x00},   // a = 0x61
    {0x41, 0x7F, 0x3F, 0x48, 0x48, 0x78, 0x30, 0x00},   // b = 0x62
    {0x38, 0x7C, 0x44, 0x44, 0x6C, 0x28, 0x00, 0x00},   // c = 0x63
    {0x30, 0x78, 0x48, 0x49, 0x3F, 0x7F, 0x40, 0x00},   // d = 0x64
    {0x38, 0x7C, 0x54, 0x54, 0x5C, 0x18, 0x00, 0x00},   // e = 0x65
    {0x48, 0x7E, 0x7F, 0x49, 0x03, 0x02, 0x00, 0x00},   // f = 0x66
    {0x98, 0xBC, 0xA4, 0xA4, 0xF8, 0x7C, 0x04, 0x00},   // g = 0x67
    {0x41, 0x7F, 0x7F, 0x08, 0x04, 0x7C, 0x78, 0x00},   // h = 0x68
    {0x00, 0x44, 0x7D, 0x7D, 0x40, 0x00, 0x00, 0x00},   // i = 0x69
    {0x60, 0xE0, 0x80, 0x80, 0xFD, 0x7D, 0x00, 0x00},   // j = 0x6A
    {0x41, 0x7F, 0x7F, 0x10, 0x38, 0x6C, 0x44, 0x00},   // k = 0x6B
    {0x00, 0x41, 0x7F, 0x7F, 0x40, 0x00, 0x00, 0x00},   // l = 0x6C
    {0x7C, 0x7C, 0x18, 0x38, 0x1C, 0x7C, 0x78, 0x00},   // m = 0x6D
    {0x7C, 0x7C, 0x04, 0x04, 0x7C, 0x78, 0x00, 0x00},   // n = 0x6E
    {0x38, 0x7C, 0x44, 0x44, 0x7C, 0x38, 0x00, 0x00},   // o = 0x6F
    {0x84, 0xFC, 0xF8, 0xA4, 0x24, 0x3C, 0x18, 0x00},   // p = 0x70
    {0x18, 0x3C, 0x24, 0xA4, 0xF8, 0xFC, 0x84, 0x00},   // q = 0x71
    {0x44, 0x7C, 0x78, 0x4C, 0x04, 0x1C, 0x18, 0x00},   // r = 0x72
    {0x48, 0x5C, 0x54, 0x54, 0x74, 0x24, 0x00, 0x00},   // s = 0x73
    {0x00, 0x04, 0x3E, 0x7F, 0x44, 0x24, 0x00, 0x00},   // t = 0x74
    {0x3C, 0x7C, 0x40, 0x40, 0x3C, 0x7C, 0x40, 0x00},   // u = 0x75
    {0x1C, 0x3C, 0x60, 0x60, 0x3C, 0x1C, 0x00, 0x00},   // v = 0x76
    {0x3C, 0x7C, 0x70, 0x38, 0x70, 0x7C, 0x3C, 0x00},   // w = 0x77
    {0x44, 0x6C, 0x38, 0x10, 0x38, 0x6C, 0x44, 0x00},   // x = 0x78
    {0x9C, 0xBC, 0xA0, 0xA0, 0xFC, 0x7C, 0x00, 0x00},   // y = 0x79
    {0x4C, 0x64, 0x74, 0x5C, 0x4C, 0x64, 0x00, 0x00},   // z = 0x7A
    {0x08, 0x08, 0x3E, 0x77, 0x41, 0x41, 0x00, 0x00},   // { = 0x7B
    {0x00, 0x00, 0x00, 0x77, 0x77, 0x00, 0x00, 0x00},   // | = 0x7C
    {0x41, 0x41, 0x77, 0x3E, 0x08, 0x08, 0x00, 0x00},   // } = 0x7D
    {0x02, 0x03, 0x01, 0x03, 0x02, 0x03, 0x01, 0x00},   // ~ = 0x7E
};

// Helper function for writing commands or pixel data.
inline bool write_cmd(I2cGeneric* i2c, const Opcode& cmd) {
    return i2c->write(DEV_ADDR, cmd.nbytes, cmd.data, 0, 0, 0);
}
inline bool write_dat(I2cGeneric* i2c, const FontCharacter& ch) {
    return i2c->write(DEV_ADDR, 1, 0x40, 8, ch.cols, 0);
}

Ssd1306::Ssd1306(satcat5::cfg::I2cGeneric* i2c)
    : m_i2c(i2c)
    , m_step(0)
    , m_page(0)
{
    // Wait 100 msec for power-on-reset, then load startup sequence.
    timer_once(100);
}

void Ssd1306::reset()
{
    // Reset internal state and immediately reload startup sequence.
    m_step = 0;
    m_page = 0;
    timer_event();
}

bool Ssd1306::display(const char* text)
{
    // Are we already busy?
    if (m_step < SSD1306_STEP_DONE) return false;

    // Convert each raw character to a font index.
    bool str_done = false;
    for (unsigned a = 0 ; a < sizeof(m_text) ; ++a) {
        if (str_done || text[a] == 0) {
            str_done = true;    // Stop reading input
            m_text[a] = 0;      // Emit blank space
        } else if (SSD1306_FONT_MIN <= text[a] && text[a] <= SSD1306_FONT_MAX) {
            m_text[a] = (u8)(text[a] - SSD1306_FONT_MIN);
        } else {
            m_text[a] = 0;      // Unknown character = blank
        }
    }

    // Start the refresh process.
    m_step = SSD1306_STEP_DRAW;
    timer_event();
    return true;
}

void Ssd1306::timer_event()
{
    // Keep trying to load commands until we're finished or
    // the I2C queue is filled.  Once done, revert to idle.
    while (next()) {                // Able to load command?
        if (++m_step >= SSD1306_STEP_DONE) {
            m_page = 1 - m_page;    // Toggle page buffer
            timer_stop();           // Stop polling timer
            return;                 // End command loop
        }
    }

    // If we reach this point, wait a moment and try again.
    timer_every(1);
}

bool Ssd1306::next()
{
    if (m_step < SSD1306_STEP_DRAW) {
        // Initial startup sequence.
        return write_cmd(m_i2c, SSD1306_STARTUP[m_step]);
    } else if (m_step == SSD1306_STEP_DRAW + 0) {
        // Set write pointer (part 1)
        return write_cmd(m_i2c, SSD1306_PAGE_RESET);
    } else if (m_step == SSD1306_STEP_DRAW + 1) {
        // Set write pointer (part 2)
        return write_cmd(m_i2c, m_page ? SSD1306_PAGE_START4 : SSD1306_PAGE_START0);
    } else if (m_step < SSD1306_STEP_SWAP) {
        // Load font data into the frame buffer.
        u8 idx = m_text[m_step - SSD1306_STEP_DATA];
        return write_dat(m_i2c, SSD1306_FONT[idx]);
    } else {
        // Final step is to swap display to the new page.
        return write_cmd(m_i2c, m_page ? SSD1306_PAGE_END4 : SSD1306_PAGE_END0);
    }
}
